#include "uart.h"
#include <stdio.h>
using namespace std;

UART::UART():Serial()
{
    ros::NodeHandle nh;
    nh.param<std::string>("Port",_port,"/dev/ttyUSB0");
    nh.param<int>("Baudrate",_baudrate,57600);
    _to = Timeout::simpleTimeout(100);

}

void UART::hdg_Callback(const std_msgs::Float64 &msg)
{
    gps_hdg = msg.data;
    // std::cout<<"=====degree gps : "<<gps_hdg<<std::endl;
}

void UART::attack_state_cb(const std_msgs::Float32MultiArray &msg)
{
    attack_state = int(msg.data[0]);
    vehicle_altitude = msg.data[4];
    vehicle_yaw = msg.data[5];
}
void UART::gpio_cb(const std_msgs::Int32 &msg)
{
    // if(ros::Time::now().toSec()-last_receive_time<0.2)
    // {
    //     return;
    // }

    // last_receive_time = ros::Time::now().toSec();
    int io_rec = msg.data;
    std::cout<<"--->gpio  : "<<io_rec<<std::endl;
    switch(io_rec)
    {
        case -138:
        gpio_array[0] = 1;
        break;
        case 138:
        gpio_array[0] = 0;
        break;
        case -122:
        gpio_array[1] = 1;
        break;
        // case 122:
        // gpio_array[1] = 0;
        // break;
        case -123:
        gpio_array[2] = 1;
        break;
        case 123:
        gpio_array[2] = 0;
        break;
        case -47:
        gpio_array[3] = 1;
        flag_47_time = ros::Time::now().toSec();
        break;
        // case 47:
        // gpio_array[3] = 0;
        // break;
    }
    
    // int gpio_int = gpio_array[0]*1+gpio_array[1]*10+gpio_array[2]*100+gpio_array[3]*1000;
    // std::cout<<"---->gpio number: "<<gpio_int<<std::endl;

    

}


void UART::imu_Callback(const sensor_msgs::Imu &msg)
{
    current_angle.x= msg.orientation.x;
    current_angle.y= msg.orientation.y;
    current_angle.z= msg.orientation.z;
    current_angle.w= msg.orientation.w;
    curr_angle = toEulerAngle(current_angle);
    sin_pitch_angle = abs(sin(curr_angle.y));
    if(sin_pitch_angle<0.1)
    {
        sin_pitch_angle = 0.1;
    }
}

void UART::cam_target_cb(const swarm_msgs::BoundingBoxes &msg)
{
    Bias_H = (msg.bounding_boxes[0].xmax+msg.bounding_boxes[0].xmin)/2.0-(1920/2); 
    Bias_V = (msg.bounding_boxes[0].ymax+msg.bounding_boxes[0].ymin)/2.0-(1080/2);
    // float box_h = msg.bounding_boxes[0].xmax-msg.bounding_boxes[0].xmin;
    // float box_v = msg.bounding_boxes[0].ymax-msg.bounding_boxes[0].ymin;
    // float box_size = sqrt(box_h * box_h + box_v * box_v);
    // float REALSIZE = 0.5;
    // float FOCUS = 325;
    // distance = REALSIZE * sqrt(Bias_H*Bias_H + Bias_V*Bias_V + FOCUS*FOCUS) /  box_size;
    
    distance = abs(vehicle_altitude/sin_pitch_angle);

    cam_update_time = ros::Time::now().toSec();
}


geometry_msgs::Vector3 UART::toEulerAngle(geometry_msgs::Quaternion quat)
{
    geometry_msgs::Vector3 ans;

    tf::Matrix3x3 R_FLU2ENU(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
    R_FLU2ENU.getRPY(ans.x, ans.y, ans.z);
    return ans;
}


void UART::RunMain()
{   
    gpio_array.reserve(4);
    gpio_array[0] = 0;
    gpio_array[1] = 0;
    gpio_array[2] = 0;
    gpio_array[3] = 0;
    attack_state = 0;
    flag_47_time = 0;
    last_receive_time = 0;
    ros::Rate loop_rate(30);
    ros::NodeHandle nh2;
    // ros::Subscriber _local_pos_sub2 = nh2.subscribe("/zhou/target_gps",10,&UART::odometry_callback,this);
    ros::Subscriber _local_odom_sub2 = nh2.subscribe("/mavros/imu/data",10,&UART::imu_Callback,this);
    ros::Subscriber _gps_global_sub2 = nh2.subscribe("/mavros/global_position/compass_hdg",10,&UART::hdg_Callback,this);
    ros::Subscriber _cam_target_sub2 = nh2.subscribe("/yindao/capture_drone", 10, &UART::cam_target_cb,this);
    ros::Subscriber _gpio_sub2 = nh2.subscribe("/gpio_event", 10, &UART::gpio_cb,this);
    ros::Subscriber _state_sub2 = nh2.subscribe("/zhou/attack_state", 10, &UART::attack_state_cb,this);
    
    
    ros::Publisher _global_pub2 = nh2.advertise<sensor_msgs::NavSatFix>("/vehicle/position/global",10);
    ros::Publisher _msg_pub2 = nh2.advertise<geometry_msgs::PoseStamped>("/zhou/msg_sendto_drones",10);


    while(ros::ok())
    {
        if(ros::Time::now().toSec()-cam_update_time>0.5)
        {
            Bias_H=0;
            Bias_V=0;
        }
        geometry_msgs::PoseStamped msg_tosend;
        msg_tosend.pose.orientation.x = curr_angle.x;
        msg_tosend.pose.orientation.y = curr_angle.y;
        // msg_tosend.pose.orientation.z = gps_hdg;
        msg_tosend.pose.orientation.z = curr_angle.z;
        msg_tosend.pose.orientation.w = distance;

        msg_tosend.pose.position.x = Bias_H;
        msg_tosend.pose.position.y = Bias_V;

        if(ros::Time::now().toSec()-flag_47_time>5)
        {
            gpio_array[3] = 0;
        }
        int gpio_int = gpio_array[0]*1+gpio_array[1]*10+gpio_array[2]*100+gpio_array[3]*1000;
        std::cout<<"---->gpio number: "<<gpio_int<<std::endl;
        msg_tosend.pose.position.z = gpio_int;
        _msg_pub2.publish(msg_tosend);
        
        sensor_msgs::NavSatFix global_test;
        global_test.position_covariance[2] = vehicle_yaw;
        global_test.position_covariance[3] = curr_angle.z;
        global_test.position_covariance[4] = attack_state;
        global_test.position_covariance[5] = distance;
        // std::cout<<"---->distance: "<<distance<<std::endl;
        _global_pub2.publish(global_test);
        ros::spinOnce();
        loop_rate.sleep();
    }
}
